//
//  Matrix.h
//  TrackingSim
//
//  Created by Garrett Manka on 2/17/12.
//  Copyright (c) 2012. All rights reserved.
//

#ifndef TrackingSim_Matrix_h
#define TrackingSim_Matrix_h

#include <math.h>

template< int R , int C >
class Matrix
{
public:
    
    Matrix();
    Matrix( double val );
    
    inline double& operator()(int row, int col);
    inline const double& operator()(int row, int col) const;
    
    void set_to_identity();
    void set_to_zero();
    Matrix<C,R> tp() const;
    
    inline double* getArray();
    
    const Matrix<R,C>& operator=(const Matrix<R,C>& rhs );
    
private:
    
    double m[R][C];
};

template< int R , int C >
Matrix<R,C>::Matrix()
{
}

template< int R , int C >
Matrix<R,C>::Matrix( double val)
{
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            m[row][col] = val;
        }
    }
}

template< int R , int C >
inline double* Matrix<R,C>::getArray()
{
    return &m[0][0];
}

template< int R, int C >
inline double& Matrix<R,C>::operator()(int row, int col)
{
    return m[row][col];
}

template< int R, int C >
inline const double& Matrix<R,C>::operator()(int row, int col) const
{
    return m[row][col];
}

template< int R, int C>
const Matrix<R, C>& Matrix<R,C>::operator=(const Matrix<R, C>& rhs)
{
    if( this == &rhs )
    {
        return *this;
    }
    
    for( int i = 0 ; i < R ; ++i )
    {
        for( int j = 0 ; j < C ; ++j )
        {
            m[i][j] = rhs(i,j);
        }
    }
    
    return *this;
}

template< int R , int C >
Matrix<C,R> Matrix<R,C>::tp() const
{
    Matrix<C,R> transpose;
    
    for( int i = 0 ; i < R ; ++i )
    {
        for( int j = 0 ; j < C ; ++j )
        {
            transpose(j,i) = m[i][j];
        }
    }
    
    return transpose;
}

template < int R , int C >
Matrix<R,C> operator+(const Matrix<R,C>& A, const Matrix<R,C>& B)
{
    Matrix<R,C> c;
    
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            c(row,col) = A(row,col) + B(row,col);
        }
    }
    
    return c;
}

template < int R , int C >
Matrix<R,C> operator-(const Matrix<R,C>& A, const Matrix<R,C>& B)
{
    Matrix<R,C> c;
    
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            c(row,col) = A(row,col) - B(row,col);
        }
    }
    
    return c;
}

template< int A_R, int A_C, int B_R, int B_C >
Matrix< A_R , B_C> operator*( const Matrix<A_R,A_C>& A, const Matrix<B_R,B_C>& B )
{
    Matrix<A_R,B_C> c;
    
    for( int row_A = 0 ; row_A < A_R ; ++row_A )
    {     
        for(int col_B = 0 ; col_B < B_C ; ++col_B )
        {
            double inner_product = 0;
            
            for( int element_index = 0 ; element_index < A_C ; ++element_index )
            {
                inner_product += A(row_A,element_index) * B(element_index,col_B);
            }
            
            c(row_A,col_B) = inner_product;
        }
    }
    
    return c;
}

template < int R , int C >
Matrix<R,C> operator*(double s , const Matrix<R,C>& m)
{
    Matrix<R,C> c;
    
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            c(row,col) = s * m(row,col);
        }
    }
    
    return c;
}

template< int R , int C >
void Matrix<R,C>::set_to_zero()
{
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            m[row][col] = 0.0;
        }
    }   
}

template< int R , int C >
void Matrix<R,C>::set_to_identity()
{
    for(int row = 0 ; row < R ; ++row)
    {
        for(int col = 0 ; col < C ; ++col )
        {
            if( row == col )
            {
                m[row][col] = 1.0;
            }
            else
            {
                m[row][col] = 0.0;
            }
        }
    }   
}

int GetMinor(double **src, double **dest, int row, int col, int order);
double CalcDeterminant( double **mat, int order);

template< int R >
void invertMatrix( const Matrix<R,R>& mat, Matrix<R,R>& inv)
{
    if( R > 1 )
    {
        //Allocate A
        double** A;
        A = new double*[R];
        
        for( int i = 0 ; i < R ; ++i )
        {
            A[i] = new double[R];
        }
        
        //Copy the matrix to A
        for( int i = 0 ; i < R ; ++i )
        {
            for( int j = 0 ; j < R ; ++j )
            {
                A[i][j] = mat(i,j);
            }
        }
        
        // get the determinant of A
        double det = 1.0/CalcDeterminant(A,R);
        
        // memory allocation
        double *temp = new double[(R-1)*(R-1)];
        double **minor = new double*[R-1];
        
        for(int i = 0 ; i < R-1 ; ++i )
        {
            minor[i] = temp + ( i * (R-1) );
        }
        
        for(int j = 0 ; j < R ; ++j )
        {
            for(int i = 0 ; i < R ; ++i)
            {
                // get the co-factor (matrix) of A(j,i)
                GetMinor(A, minor, j, i, R);
                
                inv(i,j) = det * CalcDeterminant(minor, R-1);
                
                if( ( i + j ) % 2 == 1)
                {
                    inv(i,j) = -inv(i,j);
                }
            }
        }
        
        // release memory
        delete [] minor[0];
        delete [] minor;
        
        for( int i = 0 ; i < R ; ++i )
        {
            delete[] A[i];
        }
        
        delete[] A;
    }
    else
    {
        inv(0,0) = 1.0 / mat(0,0);
    }
}

#endif
